using System;
using System.Collections.Generic;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Audio;
using Microsoft.Xna.Framework.GamerServices;
using Microsoft.Xna.Framework.Graphics;
using Microsoft.Xna.Framework.Input;
using Microsoft.Xna.Framework.Storage;
using Microsoft.Xna.Framework.Content;
//using KLib.Graphics;
using KLib.Physics.Collisions;

namespace KLib.Physics
{
    /// <summary>
    /// Manages all Physics in the game. You can attatch Graphic Elements to PhysicsObjects and have physics applied
    /// to the Graphic Elements.
    /// 
    /// This Manager DOES NOT draw the objects, it simply updates their values with physics applied.
    /// </summary>
    [RegistersService(typeof(IPhysicsService))]
    public class PhysicsManager : KLib.GameComponent, IPhysicsService
    {
        #region Fields
        private World world = new World();

        private List<Body> objects;
        private List<Geom> geometries;

        private IBroadPhaseCollider broadPhaseCollider;

        private Pool<Arbiter2> arbiterPool = new Pool<Arbiter2>(10);
        private ArbiterList arbiterList = new ArbiterList();
        internal float allowedPenitration = 0.02f;
        internal float BiasFactor = 0.8f;
        internal int maxContactsToDetect = 10;
        internal int maxContactsToResolve = 4;
        private int itterations = 5;

        #endregion

        #region Properties

        public List<Body> Bodies
        {
            get { return objects; }
        }

        public List<Geom> Geometries
        {
            get { return geometries; }
        }

        public Pool<Arbiter2> ArbiterPool
        {
            get { return arbiterPool; }
        }

        public ArbiterList ArbiterList
        {
            get { return arbiterList; }
        }
        public World World
        {
            get { return world; }
            set { world = value; }
        }

        public float AllowedPenitration
        {
            get { return allowedPenitration; }
        }

        public int MaxContactsToDetect
        {
            get { return maxContactsToDetect; }
        }
        #endregion

        #region Initialization

        public PhysicsManager(Game game)
            : base(game)
        {
            objects = new List<Body>();
        }

        public override void Initialize()
        {
            base.Initialize();
            objects = new List<Body>();

            geometries = new List<Geom>();

            broadPhaseCollider = new BruteForceCollider(this);
        }

        #endregion

        #region Methods
        public override void Update(GameTime gameTime)
        {
            arbiterList.Clear();
            float time = (float)gameTime.ElapsedGameTime.Milliseconds * 0.001f;

            foreach (Body body in Bodies)
            {
                body.Update();
            }
            DoBroadPhaseCollision();

            //--------
            collisions.Clear();
            for (int i = 0; i < arbiterList.Count; i++)
            {
                if (!collisions.Contains(arbiterList[i].bodyA))
                    collisions.Add(arbiterList[i].bodyA);
                if (!collisions.Contains(arbiterList[i].bodyB))
                    collisions.Add(arbiterList[i].bodyB);
            }
            //--------

            DoNarrowPhaseCollision(time);
            //Deal with added, removed objects
            UpdateForces(time);
            UpdateImpulses(time);
            UpdatePosition(time);

            base.Update(gameTime);
        }


        #region AddBody()
        public void AddBody(Body body)
        {
            objects.Add(body);
            RigidBody rigid = body as RigidBody;

            if (rigid != null)
            {
                geometries.AddRange(rigid.Geometry);
                rigid.Update();
            }
            body.physicsManager = this;
        }
        #endregion

        #region RemoveBody()
        public void RemoveBody(Body body)
        {
            this.objects.Remove(body);

            RigidBody rigid = body as RigidBody;
            if (rigid == null) return;
            for (int i = 0; i < rigid.Geometry.Count; i++)
            {
                this.geometries.Remove(rigid.Geometry[i]);
            }
        }

        public void RemoveAllBodies()
        {
            Bodies.Clear();
            Geometries.Clear();
        }
        #endregion

        #endregion

        #region Helper Methods

        #region UpdateForces() | UpdatePosition()
        private void UpdateForces(float time)
        {
            for (int i = 0; i < objects.Count; i++)
            {
                if (!objects[i].Enabled)
                    continue;
                objects[i].ApplyImpulses();

                if (!objects[i].IgnoresGravity)
                    objects[i].ApplyForce(-world.Gravity*objects[i].Mass);

                    objects[i].CalculateVelocity(time);

                objects[i].ClearForce();
                objects[i].ClearTorque();
            }
        }

        private void UpdatePosition(float time)
        {
            for (int i = 0; i < objects.Count; i++)
            {
                if (!objects[i].Enabled)
                    continue;

                objects[i].CalculatePosition(time);
            }
        }
        #endregion

        private void DoBroadPhaseCollision()
        {
            broadPhaseCollider.Update();
        }

        private void DoNarrowPhaseCollision(float time)
        {
            for (int i = 0; i < arbiterList.Count; i++)
			{
                arbiterList[i].Collide();
			}
            arbiterList.RemoveContactCountEqualsZero(arbiterPool);
        }

        private void UpdateImpulses(float time)
        {
            for (int i = 0; i < arbiterList.Count; i++)
            {
                arbiterList[i].PreStepImpulse(1f / time);
            }

            for (int k = 0; k < itterations; k++)
            {
                for (int i = 0; i < arbiterList.Count; i++)
                {
                    arbiterList[i].ApplyImpulse();
                }
            }
        }
        #endregion

        #region DEBUG
        public List<RigidBody> collisions = new List<RigidBody>();
        #endregion
    }
}